/* software indexer */
/* all of the functions must break if emStop() is invoked */

#include <hidef.h>      /* common defines and macros */
#include <mc9s12dt256.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dt256b"

#include "termio.c"


#define AIM_MIN_STEP_DELAY 10 /* at fastest step rate */
#define AIM_MAX_STEP_DELAY 400 /* at slowest step rate */
#define AIM_RAMP_STEPS 200 /* the number of steps that should be used to ramp up/down */
#define AIM_EXPECTED_RANGE 1000 /* expected number of steps for slightly more than the entire range of motion */

#define RAISE_MIN_STEP_DELAY 20 /* at fastest step rate */
#define RAISE_MAX_STEP_DELAY 200 /* at slowest step rate */
#define RAISE_RAMP_STEPS 3 /* the number of steps that should be used to ramp up/down */
#define RAISE_EXPECTED_RANGE 800 /* expected number of steps for slightly more than the entire range of motion */

/* define port pins */
#define A_CLK	PORTA_BIT0 /* pull high (active) to step */
#define A_DIR	PORTA_BIT1 /* 1 = CW, 0 = CCW */
#define R_CLK	PORTA_BIT2 /* pull high (active) to step */
#define R_DIR	PORTA_BIT3 /* 1 = CW, 0 = CCW */
#define R_ONF	PORTA_BIT4 /* 1 = ON, 0 = OFF */
#define RLY		PORTA_BIT5 /* 1 = ON, 0 = OFF */

#define SCALER 1 /* some scaler for step delay for adjustment */

enum direction { CCW, CW }; /* directions are when facing the shaft of the motor */

/* motor status structure */
struct motstat
{
	int enabled;        /* TRUE/1 if drive enabled */
	int indexed;        /* TRUE/1 if indexed */
	int curPos;         /* indexed position if initialized/index. Low positions are toward CCW */
	int maxPos;         /* the maximum index position possible set after indexing. Min = 0 */
	float curStepDelay;   /* current inter-step delay in milliseconds */
	float minStepDelay;   /* smallest step delay (at max speed) possible */
	float maxStepDelay;   /* largest step delay (at min speed) possible (speed to start the motor at) */
	int rampSteps;      /* number of steps that should be used to ramp up/down */ 
};

int running; /* entire indexer is enabled and running when true. set to false for disable/emStop */

struct motstat AIM;
struct motstat RAISE;

int retval;
int i, j, mt, newDelay;
int A, B, C, D, dir;

char tempchar;

/** photo interrupter booleans */
int PI1, PI2, PI3, PI4, PI5;
/**
1 = raise, rear most
2 = raise, center/bottom
3 = raise, front most
4 = aim, left most
5 = aim, right most
*/

/* forward declarations */
int indexMotors();
int setPos(struct motstat *motor, int newPos, int (*measure)(void));
int manualStep(struct motstat *motor, int dir);
int getPos(struct motstat *motor);
int setRaiseDrive(int enable);
int getRaiseDrive();
int stepMotor(struct motstat *motor, int dir);
void swdelay(float delay);
int emStop();
void sendString(char *SCIstring);
void sendInt(int integer);


void measuretest();

float testdelayvalues[800];
int testindex;

/*******************************************************
 * Test function
 ******************************************************/
void main(void)
{
  TERMIO_Init();

  /* set starting statuses for motors */
  
	AIM.enabled = TRUE; /* AIM motor always enabled */
	AIM.indexed = FALSE;
	AIM.curPos = -1; /* not indexed */
	AIM.maxPos = -1; /* not indexed */
	AIM.curStepDelay = AIM_MAX_STEP_DELAY; /* for starters */
	AIM.minStepDelay = AIM_MIN_STEP_DELAY;
	AIM.maxStepDelay = AIM_MAX_STEP_DELAY;
	AIM.rampSteps = AIM_RAMP_STEPS;
	                                                                                                         
	RAISE.enabled = FALSE;
	RAISE.indexed = FALSE;
	RAISE.curPos = -1; /* not indexed */
	RAISE.maxPos = -1; /* not indexed */
	RAISE.curStepDelay = RAISE_MAX_STEP_DELAY; /* for starters */
	RAISE.minStepDelay = RAISE_MIN_STEP_DELAY;
	RAISE.maxStepDelay = RAISE_MAX_STEP_DELAY;
	RAISE.rampSteps = RAISE_RAMP_STEPS;
	
	
	/* set port A to all outputs */
  DDRA = 0xFF;
	
	/* set outputs low to initialize */
	PORTA = 0x00;
 
  
  /* TEST(S) */
  sendString("starting test\n");
  
  /* set up for test */
  running = TRUE;
  AIM.enabled = TRUE;
  AIM.indexed = TRUE;
  AIM.curPos = 0;
  AIM.maxPos = 1000;   

  
  /* tests */
  testindex = 0;    
  setPos(&AIM, 800, NULL);
  sendString("test complete\n");

	for(;;) {} /* wait forever */
  	/* make sure that we don't leave this function */
}



/*******************************************************
 * Initialize the motor and creates index
 * Creates index
 ******************************************************/
int indexMotors()
{
/**
1 = raise, rear most
2 = raise, center/bottom
3 = raise, front most
4 = aim, left most
5 = aim, right most
*/
  
  /* invalidate current indices/indexed status */
	AIM.indexed = FALSE;
	AIM.curPos = -1; /* not indexed */
	AIM.maxPos = -1; /* not indexed */
	AIM.curStepDelay = AIM_MAX_STEP_DELAY; /* reset for slowest */
	                                                                                                         
	RAISE.indexed = FALSE;
	RAISE.curPos = -1; /* not indexed */
	RAISE.maxPos = -1; /* not indexed */
	RAISE.curStepDelay = RAISE_MAX_STEP_DELAY; /* reset for slowest */
  
  
  /* begin with AIMING motor */
  
  /* check for existing photointerrupter */
  
    /* if 4, move right until cleared */
    i = 0;
    while(PI4)
    {
      stepMotor(&AIM, CW); /* step */
      i++; /* record steps moved */
      
      /* if moved out of range, ERROR */
      if(i > AIM_EXPECTED_RANGE)
      {
        /* ERROR */
      }
    }
    
    /* if 5, move left until cleared  */
    i = 0;
    while(PI5)
    {
      stepMotor(&AIM, CCW); /* step */
      i++; /* record steps moved */
      
      /* if moved out of range, ERROR */
      if(i > AIM_EXPECTED_RANGE)
      {
        /* ERROR */
      }
    }
    
  /* find 4 */
    
    /* while 4 not found, move left */
    i = 0;
    while(PI4 != 0)
    {
      stepMotor(&AIM, CW); /* step */
      i++; /* record steps moved */
      
      /* if moved out of range, ERROR */
      if(i > AIM_EXPECTED_RANGE)
      {
        /* ERROR */
      }
    }
     
      
    /* 4 is found now */
    
      /* curPos = 0 */
      AIM.curPos = 0;
      
      /* while 5 not found, move right and increment curPos */
      i = 0;
      while(PI5 != 0)
      {
        stepMotor(&AIM, CCW); /* step */
        i++; /* record steps moved */
        AIM.curPos++; /* increment position */
        
        /* if moved out of range, ERROR */
        if(i > AIM_EXPECTED_RANGE)
        {
          /* ERROR */
        }
      }
      
        
      /* if 5 found */
      if(PI5)
      {             
        /* maxPos = curPos */
        AIM.maxPos = AIM.curPos;
        AIM.indexed = TRUE;
      }
        
        
  /* RAISING motor */ 
  
  /* enable RAISE drive */
  /*setRaiseDrive(TRUE);        */
  
	

	 return 0;
}


/*******************************************************
 * Set motor position
 * Moves motor to an index position using trapezoidal ramping (/ \)
 * Motor must be indexed, returns -1 if not.
 * measure is the pointer to the measure function, null if no measuring
 ******************************************************/
int setPos(struct motstat *motor, int newPos, void (*measure)(void))
{
  retval = -1;
	
	/* while running*/


		/* motor must be enabled */
		if(!motor->enabled)
		{
			/* DEBUG motor not enabled! */
		}
		
		else if(!motor->indexed)
		{
			/* DEBUG motor not indexed! */
		}
		
		/* else motor is enabled, indexed, and there is something to do */
		else if(!(motor->curPos == newPos))
		{	  
				
			/* proceed to move the motor to the next stepping position */
			/* reset delay */
			motor->curStepDelay = motor->maxStepDelay;
		
			/* move the motor clockwise, if > 0 then new position is CW from curPos */
			if((newPos - motor->curPos) > 0)
			{
			
  			/* first take a measurement if the pointer is valid (something is pointed to) */
  		  if(measure != NULL)
  		  {
  		      (measure);
  		  }
  		  	
				
				dir = CW; /* direction bit */
				A = motor->curPos; /* starting step of motor movement/acceleration */	
				B = motor->curPos + motor->rampSteps; /* ending step of acceleration, constant stepping here on */
				C = newPos - motor->rampSteps; /* ending constant stepping, begin of deceleration */
				D = newPos; /* final step, total number of steps to take */
				
				/* while moving the motor to the correct position */
  			while(motor->curPos != newPos)
  			{
    			/* if ramping up [A,B) */
    			if(motor->curPos >= A && motor->curPos < B)
    			{
    				/* step the motor once in direction */
    				stepMotor(motor, dir);

    				/* decrement delay for future step delay */
    				motor->curStepDelay -= ((motor->maxStepDelay - motor->minStepDelay) / motor->rampSteps);
    				
    			}

    			/* if constant min delay [B,C) */
    			if(motor->curPos >= B && motor->curPos < C)
    			{
    				/* step the motor once in direction */
    				stepMotor(motor, dir);
    				
    				/* min delay, max speed */
    				motor->curStepDelay = motor->minStepDelay;
    			}

    			/* if ramping down [C, D] */
    			if(motor->curPos >= C && motor->curPos <= D)
    			{
    				/* step the motor once in direction */
    				stepMotor(motor, dir);

    				/* increment delay for future step delay */
    				motor->curStepDelay += ((motor->maxStepDelay - motor->minStepDelay) / motor->rampSteps);
    				
    			}
    			
  			}/* while moving the motor to the correct position */	
			}
			
			
			/* move the motor counterclockwise */
			else
			{
			
			  /* first take a measurement if the pointer is valid (something is pointed to) */
  		  if(measure != NULL)
  		  {
  		      (measure);
  		  }
  		  	
				dir = CCW; /* direction bit */
				A = motor->curPos; /* starting step of motor movement/acceleration */				
				B = motor->curPos - motor->rampSteps; /* ending step of acceleration, constant stepping here on */
				C = newPos + motor->rampSteps; /* ending constant stepping, begin of deceleration */
				D = newPos; /* final step, total number of steps to take */
				
			  /* while moving the motor to the correct position */
  			while(motor->curPos != newPos)
  			{
    			/* if ramping up [A,B) */
    			if(motor->curPos <= A && motor->curPos > B)
    			{
    				/* step the motor once in direction */
    				stepMotor(motor, dir);

    				/* decrement delay for future step delay */
    				motor->curStepDelay -= ((motor->maxStepDelay - motor->minStepDelay) / motor->rampSteps);
    				
    			}

    			/* if constant min delay [B,C) */
    			if(motor->curPos <= B && motor->curPos > C)
    			{
    				/* step the motor once in direction */
    				stepMotor(motor, dir);
    				
    				/* min delay, max speed */
    				motor->curStepDelay = motor->minStepDelay;
    			}

    			/* if ramping down [C, D] */
    			if(motor->curPos <= C && motor->curPos >= D)
    			{
    				/* step the motor once in direction */
    				stepMotor(motor, dir);

    				/* increment delay for future step delay */
    				motor->curStepDelay += ((motor->maxStepDelay - motor->minStepDelay) / motor->rampSteps);
    			}
    			
  			}/* while moving the motor to the correct position */	
			}
	
			
		} /* else motor is enabled */
	
	 /* while(running) */
	
	return retval;
}

/*******************************************************
 * Manual mode stepper function
 * Moves motors manually
 ******************************************************/
 int manualStep(struct motstat *motor, int dir)
 {
   /* make sure running at min speed */
   motor->curStepDelay = motor->minStepDelay;
   
   /* step it */
   stepMotor(motor, dir);

 }
 

/*******************************************************
 * Get motor position
 * Gets motor index position
 * Motor must be indexed, returns -1 if not.
 ******************************************************/
int getPos(struct motstat *motor)
{
	retval = -1;
		
	/* if indexed */
	if(motor->indexed == TRUE)
	{
		/* return position */
		retval = motor->curPos;
	}
	/* return */
	return retval;
}


/*******************************************************
 * Enable/disable drive (for raising motor only)
 * enable is TRUE/1: relay is closed and driver is turned on
 * enable is FALSE/0: driver is turned off and relay is opened
 ******************************************************/
int setRaiseDrive(int enable)
{
  retval = -1;
	
	/* if enable */
	if(enable == TRUE)
	{
		/* close relay */
		RLY = TRUE;
		/* wait for short delay */
		
		/* turn on driver */
		R_ONF = TRUE;
		
		/* update status */
		RAISE.enabled = TRUE;
	}	
		
	/* else (disable) */
	else
	{
		/* turn off driver */
		R_ONF = FALSE;
		/* wait for short delay */
		
		/* open relay */
		RLY = FALSE;
		
		/* update status */
		RAISE.enabled = FALSE;
	}
	return retval;
}

/*******************************************************
 * Get drive status (for raising motor only)
 ******************************************************/
int getRaiseDrive()
{
  retval = -1;
  
	/* return 1 if enabled, 0 if disabled */
	return RAISE.enabled;

  return retval;
}


 
/*******************************************************
 * Low level single step function
 ******************************************************/
int stepMotor(struct motstat *motor, int dir)
{
  retval = -1;
  
		
		/* set direction bit before delay to make sure driver has time */
		if(motor == &AIM)
		{
			A_DIR = dir;
		}
		else if(motor == &RAISE)
		{
			R_DIR = dir;
		}
		else
		{
			/* DEBUG bad direction! */
		}
		
		/* wait for delay */
	  swdelay(motor->curStepDelay);

		/* bring STEP high */
		if(motor == &AIM)
		{
			A_CLK = 1;
		}
		else
		{
			R_CLK = 1;
		}
		
		/* check for unexpected opto switch */

		/* wait for delay */
		swdelay(motor->curStepDelay);

		/* bring STEP low */
		if(motor == &AIM)
		{
			A_CLK = 0;
		}
		else
		{
			R_CLK = 0;
		}
		
		/* check for unexpected opto switch */
		
		/* update index */
		if(dir == CCW && motor->indexed == TRUE)
		{
		  motor->curPos--;
		}
		else if(dir == CW && motor->indexed == TRUE)
		{
		  
		  motor->curPos++;
		}
		
		testdelayvalues[testindex] = motor->curStepDelay;
		testindex++;
	
	return retval;		
	
}
    
/*******************************************************
 * Software delay function
 * Specify delay in ms, scaled by constant SCALER
 * approximate software delay in ms ~= (delay parameter)/10
 ******************************************************/
/* 1 = 119 us*/
/* 10 = 1.0305 ms */
/* 100 = 10.165 ms */
/* 2000 = 205 ms */
void swdelay(float delay)
{
  volatile unsigned int k;
  while (delay > 0)
  {
    k = 15;
      while (k > 0)
      { 
        k = k - 1;
      }
    delay = delay - 1;
  }
}


/*******************************************************
 * Emergency stop
 * To be called by emergency stop ISR
 ******************************************************/
int emStop()
{
  retval = -1;
  
	/* break running motor functions */
	
	/* disable indexer */
	running = FALSE;
	
	/* enable drive to brake, aim always enabled */
	setRaiseDrive(TRUE);
	
	/* invalidate indexes */
	AIM.indexed = FALSE;
	RAISE.indexed = FALSE;
	
	

	return retval;
}



/* measure skeleton */
void measuretest()
{
  mt = 546;
}


/*******************************************************
 * Sends a string out of SCI
 ******************************************************/
void sendString(char *SCIstring)
{
  for(j = 0; j < strlen(SCIstring); j++)
  { 
    tempchar = SCIstring[j];
    TERMIO_PutChar(tempchar);
  }
}

void sendInt(int integer)
{
   char result[100];
   int num = 24;
   sprintf( result, "%d", num ); 
   sendString(result);
    
}